System and method for collision avoidance using virtual boundaries

ABSTRACT

A system and method of collision avoidance includes determining first positions of first joints of a first repositionable arm and second positions of second joints of a second repositionable arm. Distal ends of the first and second repositionable arms are configured to support first and second instruments, respectively. The system and method further include determining first and second virtual boundaries around the first and second repositionable arms, determining an overlap between the first and second virtual boundaries, determining an overlap force on the first repositionable arm due to the overlap, mapping the overlap force to virtual torques on the first joints proximal to the overlap, determining a tip force on a distal end of the first instrument, and applying the tip force as feedback on the first instrument.

RELATED APPLICATIONS

This patent application claims priority to and the benefit of the filingdate of U.S. Provisional Patent Application 62/300,706, entitled “SYSTEMAND METHOD FOR COLLISION AVOIDANCE USING VIRTUAL BOUNDARIES” filed Feb.26, 2016, which is incorporated by reference herein in its entirety.

TECHNICAL FIELD

The present disclosure relates generally to teleoperation of deviceswith repositionable arms and more particularly to collision avoidanceusing virtual boundaries.

BACKGROUND

More and more instruments are being replaced with autonomous andsemiautonomous devices. This is especially true in the hospitals oftoday with large arrays of autonomous and semiautonomous medical devicesbeing found in operating rooms, interventional suites, intensive carewards, emergency rooms, and/or the like. For example, glass and mercurythermometers are being replaced with electronic thermometers,intravenous drip lines now include electronic monitors and flowregulators, and traditional hand-held surgical instruments are beingreplaced by computer-assisted medical devices.

These medical devices provide both advantages and challenges to thepersonnel operating them. Many of these medical devices may be capableof autonomous or semiautonomous motion of one or more repositionablearms and/or end effectors. It is also common to operate the medicaldevices via teleoperation using one or more input controls on anoperator workstation to control the motion and/or operation of therepositionable arms and/or the end effectors. Examples of such devicesinclude the da Vinci® Surgical System commercialized by IntuitiveSurgical, Inc. of Sunnyvale, Calif. When the medical device is operatedremotely from the operator workstation and/or the end effectors arebeing used in an area not directly visible to the operator, such asduring computer-assisted surgery when the end effectors are hidden bypatient anatomy, surgical drapes, and/or the like, it may complicate theoperator's ability to detect and/or avoid collisions between the one ormore repositionable arms that could result in damage to the medicaldevice, injury to a patient or other personnel, and/or failures in asterile field.

Collision avoidance between repositionable devices, such as therepositionable arms of a medical device, is typically addressed in amotion planning context. In this context, the motion plans for each ofthe repositionable arms are determined, and the future motions of therepositionable arms are evaluated to determine whether collisionsbetween the repositionable arms will occur, with appropriate correctionsto the motion plans occurring to avoid the collision. This approach isoften far from ideal for repositionable arms controlled viateleoperation because control of the repositionable arms is directed inreal-time by an operator (i.e., the operator is unable to perceive adelay between making an input and the resulting movement) so that motionplans for the repositionable arms are not known and cannot serve as thebasis for collision avoidance determinations. As an alternative tocollision avoidance, a collision detection approach may be used thatprovides feedback to the operator when a collision occurs between therepositionable arms. But, a collision detection approach suffersdisadvantages. Actual collisions often result in a poor user experiencefor the operator. And, at a minimum, collision detection between armsoften detects the collision too late to avoid damage to the sterilefield, because the sterile drapes covering the arms typically contacteach other before the repositionable arms collide, and a drape trappedbetween two colliding arms may be torn. Some hybrid collisionavoidance-collision detection systems operate by modeling large volumes,typically with planar surfaces and which are often convex in shape, thatcircumscribe the links and joints of the repositionable arms. In somecases, the links and joints of the repositionable arm are divided intosegments with each segment having its own three-dimensionalcircumscribing primitive, such as a sphere, cylinder, or box, with anoverall circumscribing volume being constructed by taking a union of thecircumscribing primitives. As the repositionable arms are operated,collision detection is applied to the circumscribing volumes so that“collisions” between the circumscribing volumes are detected and reactedto prior to an actual collision occurring. Other approaches includeproviding separate non-intersecting workspaces for each of therepositionable arms that eliminate the need for additional collisiondetection or avoidance. These hybrid approaches, however, are often tooconservative in predicting collisions as the circumscribing volumes areoften overly large for the circumscribed links and joints and theseoverly large circumscribing volumes as well as the separate workspacesoften interfere with the ability to operate the repositionable arms withseparation distances smaller than the circumscribing volumes.

Accordingly, improved methods and systems for avoid collisions betweenthe repositionable arms of computer-assisted medical devices aredesirable.

SUMMARY

Advantageously, systems and methods in accordance with the presentinvention allow the teleoperation of devices with repositionable armswith superior collision avoidance behavior over motion-planning basedcollision avoidance or collision detection approaches. In one aspect,high fidelity CAD (computer-aided drafting or computer-aided modeling)or kinematic models of the repositionable models are used to create avirtual model of each of the repositionable arms. The virtual models arethen expanded by a predetermined distance to create a virtual boundarydefining a corresponding virtual buffer zone around each of the jointsand links in the repositionable arms. As the repositionable arms arecontrolled via teleoperation, a collision engine determines when thereis an overlap between the virtual boundaries indicating that at leasttwo of the repositionable arms have moved to within a near proximity toeach other. A physical model of surface or volume interaction, such asan elastic model based on surface or volume penetration, is then appliedby a physics engine to determine a feedback force that is applied to therepositionable arms that pushes the repositionable arms apart so as toreduce or eliminate the overlap in the virtual boundaries. The feedbackforce is then mapped to changes in the forces or torques applied tothose joints of the repositionable arms that allow the repositionablearms are pushed away from a potential collision before an actualcollision occurs. In some optional aspects, the feedback force is alsomapped to haptic feedback that is applied to the input controls used bythe operator to teleoperate the repositionable arms so as to oppose theoperator's ability to command the repositionable arms toward a collisioncondition. In this way, the operator “feels” a collision in one or moreof the input controls before it occurs, and in response the operator canmove the repositionable arms away from the potential collision.

In some aspects, the use of the virtual models and the virtual boundaryto generate the feedback force provides advantages over more traditionalcollision detection approaches. For example, relying on actualcollisions between the repositionable arms, such as those detected bynoticing differences between the desired positions and the actualpositions of the repositionable arms, often generates poor operatorforce feedback results that have poor collision directionality and lowermagnitudes, which generally do not provide for good haptic feedback onthe one or more input controls. For example, it is well known that whenphysical contact between repositionable arms is used to reflect hapticfeedback in the input controls used to control those repositionable armsthat there is an attendant compromise between the fidelity andforcefulness of the haptic feedback and stability in the control of therepositionable arms. Thus, simply increasing an amplitude in the forcefed back haptically to the input controls is likely to result inunacceptable loss of stability in the control of the repositionablearms. In contrast, because in inventive aspects the feedback force isderived from an overlap in the virtual boundaries and not an actualcollision, the full feedback control path through the repositionablearms is bypassed so that control stability may be maintained even thoughstronger haptic feedback is applied. In addition, stronger collisiondirectionality can be determined and higher haptic feedback magnitudescan be obtained via appropriate scaling. This enhanced force and hapticfeedback provides additional enhancements to the operator experience sothat the shape and contours of one repositionable arm may optionally befelt in the input controls of another repositionable arm, and itoptionally allows teleoperation where the repositionable arms move as ifto “slide” along each other. (The motion is analogous to honing a knifeedge by moving the knife and a steel across one another, but the arms donot touch and instead maintain a defined separation distance.) Inaddition, the virtual boundaries allow the repositionable arms tooperate in close proximity to each other, while also avoiding actualcollisions that could damage the repositionable arms or the steriledrapes used to create the sterile field around the repositionable arms.

In some aspects, the predetermined distances used to create the virtualboundaries are controlled to allow close operation of the repositionablearms while avoiding collisions without overly limiting the range ofmotions through which the repositionable arms may be operated. In someexamples, the predetermined distance is adjusted based on the modelingand kinematic calibration of the repositionable arms, with a smallerpredetermined distance being used for repositionable arms that arecontrollable with higher accuracy. In some cases the predetermineddistance is set to approximately the same value as the kinematiccalibration for the respective links and joints of the repositionablearms. In some cases a smaller predetermined distance is used for linksand joints located more proximally to a mechanical base on therepositionable arms, and a higher predetermined distance is used forlinks and joints located more distally on the repositionable arms. Insome cases, the predetermined distance may optionally be reduced to zeroor even set to a negative value where collisions are allowed, such asnear the distal tips of end effectors and/or instruments mounted to thedistal ends of the repositionable arms. In some cases, collisionavoidance may optionally be disabled for portions of the repositionablearms where collisions are allowed. Consistent with some embodiments, acomputer-assisted medical device includes a first repositionable arm, asecond repositionable arm, a first input control, and a control unitincluding one or more processors. The first repositionable arm include aplurality of first joints, a plurality of first links, and a firstdistal end configured to support a first instrument. The secondrepositionable arm includes a plurality of second joints, a plurality ofsecond links, and a second distal end configured to support a secondinstrument. The first input control is configured to provide movementcommands for the first instrument. The control unit is coupled to thefirst repositionable arm, the second repositionable arm, and the firstinput control. The control unit determines first positions of theplurality of first joints and second positions of the plurality ofsecond joints, uses a first virtual model of the first repositionablearm and the first positions to determine a plurality of first virtualboundaries around the first repositionable arm, uses a second virtualmodel of the second repositionable arm and the second positions todetermine a plurality of second virtual boundaries around the secondrepositionable arm, determines a first overlap between a first one ofthe plurality of first virtual boundaries and a first one of theplurality of second virtual boundaries, determines a first overlap forceon the first repositionable arm due to the first overlap, maps the firstoverlap force to first virtual torques on multiple first joints proximalto the first overlap, determines a first tip force on a distal end ofthe first instrument based on the first virtual torques, and applies thefirst tip force as a first feedback force on the first instrument andthe first repositionable arm.

Consistent with some embodiments, a method of collision avoidanceperformed by a control unit includes determining positions of aplurality of first joints of a first repositionable arm and determiningsecond positions of a plurality of second joints of a secondrepositionable arm. A first instrument is mounted to a distal end of thefirst repositionable arm. A second instrument is mounted to a distal endof the second repositionable arm. The method further includesdetermining positions of a plurality of first virtual boundaries aroundthe first repositionable arm using the first positions and a firstvirtual model of the first repositionable arm, determining positions ofa plurality of second virtual boundaries around the secondrepositionable arm using the second positions and a second virtual modelof the second repositionable arm, determining a first overlap between afirst one of the first virtual boundaries and a first one of the secondvirtual boundaries, determining a first overlap force on the first andsecond repositionable arms due to the first overlap, mapping the firstoverlap force to first virtual torques on first joints of the pluralityof first joints proximal to a first overlap position of the firstoverlap, determining a first tip force on a distal end of the firstinstrument based on the first virtual torques, applying the first tipforce as a first feedback force on the first instrument and the firstrepositionable arm, and applying the first tip force as a first hapticfeedback force to a first input control configured to provide movementcommands for the first instrument.

Consistent with some embodiments, a computer-assisted manipulationdevice comprises a repositionable arm, an input control, and a controlunit comprising one or more processors. The repositionable arm has aplurality of joints, a plurality of links, and a distal end configuredto support an instrument. The input control is configured to providemovement commands for the instrument. The control unit is coupled to therepositionable arm and the input control, wherein the control unit isconfigured to: determine joint positions of the plurality of joints; usea virtual model of the repositionable arm and the joint positions todetermine a plurality of virtual boundaries, the plurality of virtualboundaries around the repositionable arm or the instrument; determine anobject virtual boundaries around an object; determine an overlap betweenthe object virtual boundary and a virtual boundary of the plurality ofvirtual boundaries; determine an overlap force on the repositionable armdue to the overlap; map the overlap force to a virtual torque on a jointof the plurality of joints; determine a tip force on a distal end of theinstrument using the virtual torque; and apply the tip force as afeedback force on the instrument and the repositionable arm.

Consistent with some embodiments, a non-transitory machine-readablemedium stores machine-readable instructions which when executed by oneor more processors associated with a computer-assisted medical deviceare adapted to cause the one or more processors to perform a method. Themethod includes determining positions of a plurality of first joints ofa first repositionable arm and determining second positions of aplurality of second joints of a second repositionable arm. A firstinstrument is mounted to a distal end of the first repositionable arm. Asecond instrument is mounted to a distal end of the secondrepositionable arm. The method further includes determining positions ofa plurality of first virtual boundaries around the first repositionablearm using the first positions and a first virtual model of the firstrepositionable arm, determining positions of a plurality of secondvirtual boundaries around the second repositionable arm using the secondpositions and a second virtual model of the second repositionable arm,determining a first overlap between a first one of the first virtualboundaries and a first one of the second virtual boundaries, determininga first overlap force on the first and second repositionable arms due tothe first overlap, mapping the first overlap force to first virtualtorques on the first joints proximal to a first position of the firstoverlap, determining a first tip force on a distal end of the firstinstrument based on the first virtual torques, applying the first tipforce as a first feedback force on the first instrument and the firstrepositionable arm, and applying the first tip force as a first hapticfeedback force to a first input control configured to provide movementcommands for the first instrument.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG. 1 is a simplified diagram of a computer-assisted system accordingto some embodiments.

FIGS. 2A and 2B are simplified diagrams of portions of tworepositionable arms with virtual boundaries according to someembodiments.

FIG. 3 is a simplified diagram of portions of two repositionable armswith virtual boundaries having multiple overlaps according to someembodiments.

FIG. 4 is a simplified diagram of a method of generating a virtual modelfor a repositionable arm according to some embodiments.

FIG. 5 is a simplified diagram of a method of collision avoidance usingvirtual boundaries according to some embodiments.

In the figures, elements having the same designations have the same orsimilar functions.

DETAILED DESCRIPTION

In the following description, specific details are set forth describingsome embodiments consistent with the present disclosure. It will beapparent to one skilled in the art, however, that some embodiments maybe practiced without some or all of these specific details. The specificembodiments disclosed herein are meant to be illustrative but notlimiting. One skilled in the art may realize other elements that,although not specifically described here, are within the scope and thespirit of this disclosure. In addition, to avoid unnecessary repetition,one or more features shown and described in association with oneembodiment may be incorporated into other embodiments unlessspecifically described otherwise or if the one or more features wouldmake an embodiment non-functional. The term “including” means includingbut not limited to, and each of the one or more individual itemsincluded should be considered optional unless otherwise stated.Similarly, the term “may” indicates that an item is optional.

FIG. 1 is a simplified diagram of a computer-assisted system 100according to some embodiments. As shown in FIG. 1, computer-assistedsystem 100 includes a device 110 with one or more repositionable arms120. In some examples, each of the one or more repositionable arms 120may optionally include one or more links and one or more joints with theone or more joints allowing articulation of the one or more links. Insome examples, each of the one or more repositionable arms 120 mayoptionally include one or more flexible members, such as a steerabletube. Each of the one or more repositionable arms 120 is configured tosupport one or more end effectors 125 that may optionally be mounted toa distal end of a respective one of the repositionable arms 120. Anassembly of a repositionable arm 120 with a mounted end effector 125 canbe termed an “arm-and-end-effector assembly.” In some examples, device110 may be consistent with a computer-assisted manipulation device, suchas a computer-assisted medical device. A specific example of acomputer-assisted medical device is a teleoperated surgical device. Theone or more end effectors 125 include surgical instruments, imagingdevices, and/or the like. In some examples, the surgical instrumentsinclude clamps, grippers, retractors, cautery tools, suction tools,suturing devices, and/or the like. In some examples, the imaging devicesinclude stereo- and mono-scopic imaging devices, imaging devices in thevisible and infrared ranges, steerable endoscopic imaging devices,and/or the like. In some examples, each of the repositionable arms 120and/or end effectors 125 may optionally be configured with a remotecenter of motion during operation. The remote center of motion is alocation that maintains a stationary position in space even though linksand/or joints of a corresponding repositionable arm 120, end effector125, or both change in orientation or angular or linear position. Forexample, the remote center of motion may remain in a stationary positioneven where an orientation of the end effector 125 relative to the remotecenter of motion has changed. In some examples, the remote center ofmotion may optionally correspond to a portion of the correspondingrepositionable arm 120 and/or end effector 125 that is inserted througha body wall of a patient during operation. In some examples, the remotecenter of motion is allowed to move within a range about a fixedlocation in space. As a specific example, where the remote center ofmotion is for a portion inserted through the body wall of patient, therange may be defined using criteria such as the flexibility and materialcharacteristics of the body wall, the amount of motion that can beaccommodated while not causing excess trauma to the patient, and thelike.

Device 110 is coupled to a control unit 130 via an input/output (I/O)interface 146. I/O interface 146 may optionally include one or morecables, connectors, ports, and/or buses, and it may optionally furtherinclude one or more networks with one or more network switching and/orrouting devices. Control unit 130 includes a processor 142 coupled tomemory 144 and to I/O interface 146. Operation of control unit 130 iscontrolled by processor 142. And although control unit 130 is shown withonly one processor 142, it is understood that processor 142 isrepresentative of one or more central processing units, multi-coreprocessors, microprocessors, microcontrollers, digital signalprocessors, field programmable gate arrays (FPGAs), application specificintegrated circuits (ASICs), and/or the like in control unit 130.Control unit 130 may optionally be implemented as a stand-alonesubsystem and/or board added to a computing device or as a virtualmachine.

Memory 144 is used to store software executed by control unit 130 and/orone or more data structures used during operation of control unit 130.Memory 144 includes one or more types of machine readable media. Somecommon forms of machine readable media may include floppy disk, flexibledisk, hard disk, magnetic tape, any other magnetic medium, CD-ROM, anyother optical medium, punch cards, paper tape, any other physical mediumwith patterns of holes, RAM, PROM, EPROM, FLASH-EPROM, any other memorychip or cartridge, and/or any other medium from which a processor orcomputer is adapted to read.

Control unit 130 is further coupled to an operator workstation 170 viaI/O interface 146. Operator workstation 170 is used by an operator, suchas a surgeon, to control the movement and/or operation of therepositionable arms 120 and/or the end effectors 125 usingteleoperation. To support teleoperation of the repositionable arms 120,operator workstation 170 includes a display system 180 for displayingimages of at least portions of one or more of the repositionable arms120 and/or end effectors 125. For example, display system 180 can beused when it is impractical and/or impossible for the operator to seethe repositionable arms 120 and/or the end effectors 125 as they arebeing used. Operator workstation 170 further includes a consoleworkspace with one or more input controls 195 (also called “mastercontrols 195”) that are usable for device 110, the repositionable arms120, and/or the end effectors 125. Each of the input controls 195 iscoupled to the distal end of their own repositionable arms so thatmovements of the input controls 195 are detected by the operatorworkstation 170 and communicated to control unit 130 through I/Ointerface 146. To provide improved ergonomics, the console workspace mayoptionally include one or more rests, such as an arm rest 190 on whichoperators may rest their arms while manipulating input controls 195. Insome embodiments, device 110, operator workstation 170, and control unit130 may optionally correspond to a da Vinci® Surgical Systemcommercialized by Intuitive Surgical, Inc. of Sunnyvale, Calif. It willbe understood that control unit 130 may optionally be separate fromdevice 110 and workstation 170, be incorporated into either device 110or workstation 170, or be distributed between device 110 and workstation170.

Referring back to control unit 130, memory 144 includes several modules,applications, data structures, and/or the like that include virtualmodels 150, a collision engine 152, a physics engine 154, a motioncontrol module 156, and a haptic feedback module 158. The virtual models150, collision engine 152, physics engine 154, and/or motion controlmodule 156 are used to support autonomous and/or semiautonomous controlof device 110, and each may optionally include one or more applicationprogramming interfaces (APIs) for receiving inputs, sensor data,instructions, and/or the like for supporting the control of device 110,including repositionable arms 120 and end effectors 125. And althoughvirtual models 150, collision engine 152, physics engine 154, motioncontrol module 156, and haptic feedback module 158 are depicted assoftware applications, each of the virtual models 150, collision engine152, physics engine 154, motion control module 156, and haptic feedbackmodule 158 may optionally be implemented using hardware, software,and/or a combination of hardware and software.

When input controls 195 are used to control end effectors 125 usingteleoperation, sensors coupled to the repositionable arms of operatorworkstation 170 are used to sense the positions, velocities, and/ororientations of input controls 195. The sensed positions, velocities,and/or orientations are then passed to control unit 130 through I/Ointerface 146, where they are processed by motion control module 156 tocompute suitable forces and/or torques for the joints of repositionablearms 120 and end effectors 125 that will teleoperate end effectors 125to corresponding positions and/or orientations that track input controls195. The computed forces and/or torques are then provided to respectivejoints in the repositionable arms 120 and end effectors 125 using I/Ointerface 146. Examples of teleoperated systems consistent with thisapproach are further described in U.S. Pat. No. 6,424,885, entitled“Camera Referenced Control in a Minimally Invasive Surgical Apparatus,”which is hereby incorporated by reference in its entirety.

Control unit 130 additionally supports collision avoidance betweenrepositionable arms 120 and/or end effectors 125 using virtualboundaries. More specifically, virtual models 150 include high fidelityCAD and/or kinematic models of the links and joints of repositionablearms 120 and/or end effectors 125 that are used to model at least thepositions of the exterior surfaces of the joints and links that make uprepositionable arms 120 and/or end effectors 125. Virtual models 150 arefurther parameterized so that as outputs from sensors associated withthe joints are input to control unit 130 using I/O interface 146,position and orientation data about the joints may be used by virtualmodels 150 to generate current positions of the exterior surfaces of thejoints and links. The accuracy attainable using virtual models 150 mayoptionally depend on several practical limitations in determining thepositions of repositionable arms that impact the ability to generategood kinematic calibration for the repositionable arms 120. For example,the accuracy of virtual models 150 is typically affected by acombination of manufacturing tolerances, calibration accuracy, sensoraccuracy, backlash, wear and dynamics of each of the repositionable arms120, and/or the like. In addition, in a repositionable arm 120, whichincludes a series of links, where each link defines a geometrictransform from a proximally coupled link to a distally coupled linkwhere the most proximal link may optionally be shared by multiplerepositionable arms and defines an origin of each of the geometrictransforms. The geometric transform defined by each link has intrinsicerrors due to variability in the mechanical assembly and manufacturingof the link and some or all of the transform may be measured for eachlink built to achieve high kinematic accuracy. In addition, where linksmeet at repositionable elements, such as joints, there is a variablegeometric transformation between one link and the next and sensors aretypically installed and calibrated to provide a true angle ordisplacement and axis of articulation. In addition, the transformdefined by each link may vary as loads are applied to the link, due forexample to flexibility in the link, and may optionally be accounted forusing a dynamic model of the link. Some factors such as backlash in thesensors and/or loose attachments between links are often challenging toestimate and degrade kinematic accuracy. Additionally, the accuracy ofthe models typically degrades for links located more distally on therepositionable arm. In some embodiments, limitations in the kinematiccalibration may optionally allow virtual models 150 to position externalsurfaces of within 6-12 mm of their actual position. Additionally,modeling accuracy to within 6-12 mm also provides reasonable clinicaloutcomes when additional positional and/or imaging feedback are used tomonitor the positions and orientations of repositionable arms 120. Insome examples, surface details or features of repositionable arms 120and/or end effectors 125 that are smaller than the modeling accuracy areoptionally simplified so that exterior surfaces of the links and/orjoints are modeled using simpler, fewer, and/or predominantly convexsurface models.

In some embodiments, one or more registration markers, fiducial markers,and/or the like mounted on repositionable arms 120 and/or end effectors125 may alternatively be tracked using one or more tracking sensors,such as an imaging device, to supplement and/or replace the sensorinputs to determine the orientations of one or more of the joints inrepositionable arms 120 and/or end effectors 125. Optionally, otherposition and shape sensing components are additionally and/oralternatively used, such as optical fiber shape sensors using FiberBragg Grating technology, such as the optical fiber shape sensorsdisclosed in U.S. Pat. No. 7,720,322 entitled “Fiber Optic ShapeSensor,” which is hereby incorporated by reference in its entirety.

Virtual models 150 further include virtual boundaries that are extendedbeyond the exterior surfaces by corresponding predetermined distances soas to create virtual boundaries with virtual buffer zones around thejoints and links of repositionable arms 120 and/or end effectors 125. Insome examples, lengths to which the virtual boundaries are extendedbeyond the exterior surfaces are selected so that any positionalinaccuracies in the actual positions of the exterior surfaces of jointsand links (e.g., due to errors from the modeling and/or kinematiccalibration limitations as discussed above) help ensure that the actualpositions of the exterior surfaces are within the virtual boundaries. Insome examples, the predetermined distances may be selected to beapproximately the same or slightly larger than the correspondingmodeling and/or kinematic calibration for the corresponding joints orlinks. In some examples, the predetermined distance may optionally belarger for joints and/or links located toward the distal ends ofrepositionable arms 120 and/or end effectors 125 to reflect thegenerally less accurate modeling and/or kinematic calibration of themore distal joints and/or links. And, the predetermined distance may beselected in whole or in part to allow sufficient space for steriledrapes that cover the repositionable arms 120 to avoid damaging contactthat might result in tearing or puncturing a drape. In some examples,the lengths to which the virtual boundaries are extended may optionallybe between 6-12 mm.

As repositionable arms 120 and end effectors 125 are manipulated and/orteleoperated using input controls 195, virtual models 150 are used todetermine current positions of the virtual boundaries. The currentpositions of the virtual boundaries are then passed to collision engine152 to determine whether there is a virtual collision between virtualboundaries of different repositionable arms 120 and/or end effectors125. In some examples, the virtual collision is detected whenever thereis an intersection or overlap between virtual boundaries of differentrepositionable arms 120 and/or end effectors 125 or alternatively due tomore than one overlap in the corresponding virtual buffer zones. Each ofthe overlaps in virtual boundaries identifies a virtual collisionbetween the corresponding repositionable arms 120 and/or end effectors125.

Each of the overlaps is then passed to physics engine 154 to determine acorresponding a virtual overlap force of the corresponding virtualcollision. In some examples, physics engine 154 determines a magnitudeof the corresponding virtual overlap force based on one or more of anamount of overlap, a depth of the overlap, a volume of the overlap, anarea of contact of the overlap, and/or the like. In some examples,physics engine 154 determines a direction of the corresponding virtualoverlap force based on a direction of a maximum overlap between therespective virtual boundaries and/or based on a surface normal of therespective virtual boundaries at the overlap. In some examples, physicsengine 154 uses a virtual spring model with a linear or alternatively anon-linear constant to determine the magnitude of the virtual overlapforce. In some examples, physics engine 154 uses a deformable materialmodel, such a gas or fluid dynamics model, for the regions within thevirtual boundaries so that the volume of the overlap in combination witha coefficient of resiliency for virtual material in the regions withinthe virtual boundaries provides a displacing force from which thevirtual overlap force is derived. Examples of models generating virtualfeedback forces are described in further detail in International PatentPublication No. WO 2015/120008 entitled “System and Method for DynamicVirtual Collision Objects,” which is hereby incorporated by reference inits entirety. In some embodiments, physics engine 154 may optionallyfurther include surface interaction effects between the overlappingvirtual boundaries based on one or more of the deformable materialmodel, damping, sliding friction, surface roughness, and/or the like. Insome examples, the surface interaction effects may optionally result infeedback forces perpendicular to the virtual overlap force that modelresistance to sliding one virtual boundary along another virtualboundary.

The virtual overlap force is then used to determine a correspondingfeedback on one or all of the respective repositionable arms, endeffectors, or arm-and-end-effector assemblies associated with thevirtual collision. Without loss of generality, this process is describedbelow for one of the respective repositionable arms, end effectors, orarm-and-end effector assemblies, which is generically referred to as therespective repositionable arm and/or end effector. It is understood thatdifferent repositionable arms and/or end effectors have differentkinematics and thus would be analyzed using their own respectiveversions of the various functions and mappings. In some examples, arespective Jacobian inverse or Jacobian pseudo-inverse for therespective repositionable arm and/or end effector between the point ofoverlap and the joints of the respective repositionable arm and/or endeffector located proximal to the point of overlap is used in thisdetermination. In some examples, a Jacobian pseudo-inverse may becalculated using a Jacobian transpose. The Jacobian inverse or Jacobianpseudo-inverse is used to map the virtual overlap force to virtual jointtorques in each of the joints that is located proximal to the point ofoverlap in the respective repositionable arm and/or end effector. Inthese examples, computation of the virtual joint torques is limited tothose joints proximal to the point of overlap. In many cases, theoverlap can be satisfactorily reduced and/or eliminated using onlyjoints proximal to the point of the virtual collision. In otherexamples, computation of the virtual joint torques are not thus limited,and are also determined for those joints not proximal to the point ofoverlap.

In some examples, the virtual joint torques are then used to determinecorresponding tip forces for the respective repositionable arm and/orend effector. A function relating the joint torques and tip forces maybe used. In some examples, an inverse of the Jacobian inverse orJacobian pseudo inverse (e.g., an inverse of the Jacobian transpose)between the tip of the respective end effector and the joints in therespective repositionable arm and/or end effector can then be used withthe virtual torques in the joints, to determine a corresponding tipforce that could be applied to a distal-most tip of the respectiverepositionable arm and/or end effector to emulate the effects of thevirtual collision. In some examples, the corresponding tip force is thenprovided to motion control module 156 to be applied as a feedback to themotion control of the respective repositionable arm and/or end effectorso that the respective repositionable arm and/or end effector opposesthe virtual collision and resists being teleoperated closer to an actualcollision. In some examples, the feedback of the corresponding tip forceis superimposed on the motion dictated by the teleoperated commands forthe respective repositionable arm and/or end effector.

In some embodiments, when more than one overlap is detected by collisionengine 152, a respective virtual overlap force for each respectiveoverlap is separately determined. The respective virtual overlap forceis then used to generate respective virtual torques for both therepositionable arms and/or end effectors associated with each respectiveoverlap. In some examples, the respective virtual torques may besuperimposed for their corresponding repositionable arms and/or endeffectors to determine an overall corresponding tip force for thecorresponding repositionable arm and/or end effector that is provided tomotion control module 156. Alternatively, the overall corresponding tipforce is determined by superimposing separate tip forces generated foreach of the overlaps.

In some embodiments, the tip force for each of the repositionable armsand/or end effectors subject to a virtual collision is then provided tohaptic feedback module 158 to provide haptic feedback to the operatorthrough respective input controls 195. In some examples, the tip forceis scaled to reflect differences in relative force used to manipulatethe repositionable arm 120 and/or end effector 125 and the force used tomanipulate the corresponding input control 195. In some examples, aninverse of a Jacobian transpose for the respective input control 195 isused to map the tip force to respective joint torques in therepositionable arm of the input control 195 to apply haptic feedback tothe operator. Alternatively, in some embodiments, differences betweenactual positions of the repositionable arms and/or end effectors andcorresponding commanded positions of the repositionable arms and/or endeffectors are used to determine the haptic feedback on each of therespective input controls 195.

Modeling the overlap forces using the virtual boundaries provides forone or more advantages to collision avoidance. First, because a virtualoverlap force is not determined until after an overlap in virtualboundaries is detected, embodiments may be configured such that theeffects of virtual collisions are only determined when correspondingrepositionable arms and/or end effectors are in close proximity to eachother and ignored at other times. Second, use of the virtual overlapenables accurate models of both (a) a severity of the virtual collisionand (b) corresponding surface normals for the virtual collision. Thesemore accurate models enable determination of more accurate directionsand magnitudes of feedback forces in the respective repositionable armsand/or end effectors for pushing the respective repositionable armsand/or end effectors away from the virtual collision. This helps toprevent actual collisions between the respective repositionable armsand/or end effectors.

According to some embodiments, various optional repositionable armand/or end effector motions may be used to resolve a virtual collisionin which one or more overlaps exist. One repositionable arm and/or endeffector may stay stationary while the second repositionable arm and/orend effector moves in any combination of one, two, or three Cartesiantranslations and orientations (the various combinations are not listedhere to avoid excessive description) to increase a respective distancebetween each of the overlaps in the direction of the respective surfacenormal. Alternatively, both of the repositionable arms and/or endeffectors may move in any combination of one, two, or three Cartesiantranslations and orientations to increase the respective distancebetween each of the overlaps in the direction of the respective surfacenormal. In some examples, the relative motion between the tworepositionable arms and/or end effectors may optionally result is asliding-type motion that resembles a first link sliding across a second,stationary link, or that resembles two links sliding across one another.In some examples, the sliding motion may optionally include a rollingmotion, so that the resulting motion resembles a first link rollingaround a second, stationary link, or two links rolling around eachother. In various embodiments, the links can be modeled as sticks.

Thus, for a link of an articulated arm and/or end effector with alongitudinal axis defined between the opposite ends of the link, thelocation of the overlap between the two respective virtual boundariesmay move along the link in a direction generally aligned with thelongitudinal axis, and/or the location of the overlap between the tworespective virtual boundaries may generally move along a curve on thesurface of the link. Similarly, the location of the overlap between thetwo respective virtual boundaries may move along the surface of eitheror both of the respective virtual boundaries boundary in a directiongenerally aligned with the longitudinal axis, or the location of theoverlap between the two respective virtual boundaries may move along acurve on the surface of one or both of the respective virtualboundaries. As a result, the two repositionable arms and/or endeffectors appear to slide with reference to each other, but with adistance between them to prevent damage to the repositionable armsand/or end effectors and/or to sterile drapes covering therepositionable arms and/or end effectors. In some examples, the operatormay optionally experience this sliding motion in one or both of theinput controls, or the sliding motion may optionally occur without theoperator experiencing any sensation in the input controls. In someembodiments, additional surface interaction effects may optionally beincluded in the feedback that results from the overlap of the virtualboundaries. In some examples, the feedback may optionally include forcesperpendicular to the virtual overlap force that model resistance to thesliding motion. In some examples, the amount of resistance to thesliding motion may be proportional to the virtual overlap forceconsistent with modeling of friction between two surfaces being slidagainst each other.

In some embodiments, other configurations and/or architectures may beused with computer-assisted system 100. In some examples, control unit130 may optionally be included as part of operator workstation 170and/or device 110. In some embodiments, computer-assisted system 100 isusable in an operating room and/or an interventional suite. And althoughcomputer-assisted system 100 is shown as including only one device 110with two repositionable arms 120, one of ordinary skill would understandthat computer-assisted system 100 may optionally include any number ofdevices with repositionable arms and/or end effectors of similar and/ordifferent design from device 110. In some examples, each of the devicesmay include one or three or more repositionable arms 120 and/or endeffectors 125.

FIGS. 2A and 2B are simplified diagrams of portions of tworepositionable arms with virtual boundaries according to someembodiments. FIG. 2A shows the two repositionable arms without anoverlap between their respective virtual boundaries and FIG. 2B showsthe same repositionable arms with an overlap between their respectivevirtual boundaries. In some examples, the two repositionable arms areconsistent with the repositionable arms 120 and/or end effectors 125 ofdevice 110.

In more detail, FIG. 2A shows a portion of a first repositionable armhaving links 210 and 220 coupled by a joint 215. The firstrepositionable arm also has a virtual boundary 230 that is extendedbeyond the exterior surfaces of links 210 and 220 and joint 215 by apredetermined distance 235. And although the first repositionable arm isshown in FIG. 2A in two dimensions, it is understood that links 210 and220 as well as joint 215 extend into three dimensions and that virtualboundary 230 extends around links 210 and 220 and joint 215 in all threedimensions. As joint 215 is manipulated (e.g., to change an anglebetween links 210 and 220) a corresponding shape of virtual boundary 230is changed accordingly. In some examples, virtual boundary 230 andpredetermined distance 235 are consistent with a virtual model of thefirst repositionable arm included in virtual models 150.

FIG. 2A also shows a portion of a second repositionable arm having links240 and 250 coupled by a joint 245. The second repositionable arm alsohas a virtual boundary 260 that is extended beyond the exterior surfacesof links 240 and 250 and joint 245 by a predetermined distance 265. Andalthough the second repositionable arm is shown in FIG. 2A in twodimensions, it is understood that links 240 and 250 as well as joint 245extend into three dimensions and that virtual boundary 260 extendsaround links 240 and 250 and joint 245 in all three dimensions. As joint245 is manipulated (e.g., to change an angle between links 240 and 250)a corresponding shape of virtual boundary 260 is changed accordingly. Insome examples, virtual boundary 260 and predetermined distance 265 areconsistent with a virtual model of the second repositionable armincluded in virtual models 150.

When the first and second repositionable arms of FIG. 2A are movedcloser to each other, an overlap between virtual boundary 230 andvirtual boundary 260 may occur, such as is depicted in FIG. 2B atoverlap 270. When overlap 270 is detected by collision engine 152,several characteristics of overlap 270 are determined. For example, eventhough virtual boundary 230 and virtual boundary 260 overlap by varyingamounts along both virtual boundary 230 and virtual boundary 260, theeffects of a virtual collision represented by overlap 270 may optionallybe represented by noting where maximum overlap between virtual boundary230 and virtual boundary 260 occurs. In some examples, collision engine152 detects the maximum overlap by finding a point on a first one of thevirtual boundaries (e.g., virtual boundary 230) that extends farthestinto the region defined by a second one of the virtual boundaries (e.g.,virtual boundary 260) and noting a distance between the first virtualboundary and the second virtual boundary along a direction of a surfacenormal for the first virtual boundary at the maximum overlap. This isshown in FIG. 2B using the double-headed arrow at overlap 270. In someexamples, the direction of the surface normal may optionally be used byphysics engine 154 to determine a direction of a corresponding virtualoverlap force, and the amount of maximum overlap is used by physicsengine 154 to determine a magnitude of the virtual overlap force (e.g.,via application of a spring model for the overlap as previouslydiscussed). In some embodiments, additional surface and/or volumeinteraction models may optionally be used to determine the directionand/or the magnitude of the corresponding virtual overlap force. In someexamples, physics engine 154 may optionally use a deformable materialmodel, such a gas or fluid dynamics model, for the regions within thevirtual boundaries so that the volume of the overlap in combination witha coefficient of resiliency for virtual material in the regions withinthe first and second virtual boundaries provides a displacing force fromwhich the virtual overlap force is derived. Examples of modelsgenerating virtual feedback forces are described in further detail inInternational Patent Publication No. WO 2015/120008 entitled “System andMethod for Dynamic Virtual Collision Objects,” which is herebyincorporated by reference in its entirety. Examination of FIG. 2breveals that the direction of greatest risk for an actual collision isalong the direction of the virtual overlap force because this is wherethe links and joints of the first and second repositionable arms areclosest together. FIG. 2B also shows that pushing the first and secondrepositionable arms apart in the direction of the virtual overlap forceis an efficient way to reduce and/or eliminate overlap 270. Once thevirtual overlap force is determined, it may be used to apply feedback toboth the first repositionable arm and the second repositionable arm aspreviously described.

FIG. 3 is a simplified diagram of portions of two repositionable armswith virtual boundaries having multiple overlaps according to someembodiments. As shown in FIG. 3, the two repositionable arms havemultiple overlaps between their respective virtual boundaries. In someexamples, the two repositionable arms are consistent with therepositionable arms 120 and/or end effectors 125 of device 110.

In more detail, FIG. 3 shows a portion of a first repositionable armhaving links 310, 320, and 330 coupled, respectively, by joints 315 and325. The first repositionable arm also has a virtual boundary 340 thatis extended beyond the exterior surfaces of links 310, 320, and 330 andjoints 315 and 325 by a predetermined distance 345. FIG. 3 also shows aportion of a second repositionable arm having links 350 and 360 coupledby a joint 355. The second repositionable arm also has a virtualboundary 370 that is extended beyond the exterior surfaces of links 350and 360 and joint 355 by a predetermined distance 375. As with therepositionable arms and virtual boundaries of FIGS. 2A and 2B, therepositionable arms and virtual boundaries of FIG. 3 extend into threedimensions and the virtual boundaries 340 and 370 change shape as joints315 and 325 and joint 355 are respectively manipulated.

As further shown in FIG. 3, when the first and second repositionablearms are moved closer to each other, multiple overlaps between virtualboundary 340 and virtual boundary 370 may occur, such as is depicted atoverlaps 380 and 390. Collision engine 152 and physics engine 154initially process each of the overlaps 380 and 390 separately in muchthe same way they processed overlap 270. Thus, for each of overlaps 380and 390 a respective virtual overlap force and a respective feedback toreduce and/or eliminate the respective overlap is determinedindependently, and it is possible that the respective feedbacks mayaffect different joints in both the first and second repositionable armsbased on the locations of the overlaps 380 and 390. Once the respectivefeedbacks are separately determined, an overall composite feedback isdetermined using superposition. The efficacy of this superpositionapproach is shown in FIG. 3. For example, under the assumption that theproximal ends of the first and second repositionable arms are located asindicated by the proximal arrows in FIG. 3, feedback provided to reduceoverlap 380 alone (e.g., feedback to push portions of the first andsecond repositionable arms apart in a mostly horizontal direction ofFIG. 3, such as feedback to cause a counter-clockwise rotation of ajoint (not shown) below joint 315 and a counter-clockwise rotation ofjoint 355) would not do much to reduce or eliminate overlap 390 andfeedback provided to reduce overlap 390 alone (e.g., feedback to pushportions of the first and second repositionable arms apart in a mostlyvertical direction of FIG. 3, such as feedback to cause acounter-clockwise rotation of joint 325 and a counter-clockwise rotationof a joint (not shown) to the left of joint 355) would not do much toreduce or eliminate overlap 380. However, by superimposing feedback witha mostly horizontal component to reduce overlap 380 and a mostlyvertical component to reduce overlap 390 is more effective at reducingand/or eliminating both overlap 380 and overlap 390. This superpositionapproach also generalizes to three or more points of overlap, and it isalso usable to additional provide feedback that occurs due to overlapsbetween the first and/or second repositionable arms and a thirdrepositionable arm (not shown).

FIG. 4 is a simplified diagram of a method of generating a virtual modelfor a repositionable arm according to some embodiments. One or more ofthe processes 410-430 of method 400 may be implemented, at least inpart, in the form of executable code stored on non-transitory, tangible,machine readable media that when run by one or more processors (e.g.,the processor 142 in control unit 130) may cause the one or moreprocessors to perform one or more of the processes 410-430. In someembodiments, method 400 is usable to create one or more data structuresand/or executable procedures to implement a corresponding model for arepositionable arm, such as any of the repositionable arms 120 and/orend effectors 125. In some embodiments, the virtual model may be one thevirtual models 150. In some embodiments, method 400 is usable to createone or more virtual models for each of the repositionable arms, endeffectors, and/or instruments used by a computer-assisted medicaldevice.

At a process 410, CAD and/or kinematic models of a repositionable armare received. The CAD and/or kinematic models should include sufficient(e.g., high fidelity) detail so that exterior surface details can bemodeled with a accuracy at least as good as an accuracy to which therepositionable arm can be controlled. In some examples, high fidelityaccuracy is one with an accuracy of 6-12 mm or less. Thus, the CADand/or kinematic models include details on the types, numbers, shapes,and/or configurations of links and joints in the repositionable arm. Insome examples, the CAD and/or kinematic models may further includeinformation on how each of the joints in the repositionable arm can bemanipulated and/or repositionable during operation of the repositionablearm. In some examples, the CAD and/or kinematic models may be generatedfrom a CAD or similar tool.

At a process 420, convex hulls are determined. Using the CAD and/orkinematic models received during process 410, the repositionable arm isdivided into segments. The segments are selected to that the exteriorsurface of each of the respective segments can be approximated within adesired accuracy using a convex hull. In some examples, a CAD tool mayoptionally be used to extract the convex hulls from the CAD and/orkinematic models. In some examples, the CAD and/or kinematic models mayoptionally include a mesh of vertices and/or control points thatapproximate the exterior surface of each of the respective segments. Insome examples, the number of vertices and/or control points used in themesh or as part of the convex hull model may optionally be selectedbased on the desired accuracy for the convex hull. In some examples, thedesired accuracy is 6-12 mm or less. In some examples, the desiredaccuracy for each of the segments may optionally vary (e.g., so thatmore proximal segments of the repositionable arm are modeled withgreater accuracy than more distal segments of the repositionable arm).In some examples, the convex hull determined for a respective segmentmay optionally represent a simplified model of the exterior surface ofthe respective segment by eliminating and/or smoothing out exteriorsurface features that have variations in dimension that are finer thanthe desired accuracy.

At a process 430, the convex hulls are expanded. In order to generate avirtual boundary for each of the segments, and the repositionable arm asa whole, each of the convex hulls determined during process 420 areexpanded in size based on a predetermined distance. In this way, thevirtual boundary provides a virtual buffer zone around the links andjoints of the repositionable arm. In some examples, the predetermineddistance is selected to be at least as accurate as the kinematiccalibration to which a motion control module, such as motion controlmodule 156, is able to position and orient the repositionable arm. Insome examples, the predetermined distance may optionally vary fordifferent segments of the repositionable arm. In some examples, segmentslocated more proximally on the repositionable arm may optionally have asmaller predetermined distance than segments located more distally onthe repositionable arm.

In some embodiments, the virtual model may optionally be parameterizedto account for changes in the positions and/or articulations in thejoints of the repositionable arm as it is operated and/or manipulated.Thus, an application or module using the virtual model is able toprovide current joint positions and/or orientations to the virtual modeland a corresponding virtual boundary that accounts for the current jointpositions and/or orientations is generated and made available for use.In some examples, the joint positions and/or orientations are providedto the virtual model using an API and/or similar call interface.

In some embodiments, the virtual model may optionally be parameterizedto designate a position along the repositionable arm at which thevirtual boundary ends. In some examples, segments of the repositionablearm located distally to the position are not used to generate thevirtual boundary so that the virtual boundary generated by the virtualmodel does not include boundary segments more distal to the position.This allows a user of the virtual model to suppress the virtual boundarymore distal to the position so that overlaps and/or virtual collisionsare not detected distal to the position. In some examples, this allowsvirtual collisions to be turned off where actual collisions between therepositionable arm and/or its end effector with another virtual armand/or end effector are permitted. In some examples, the position isselected to correspond to where the repositionable arm and/or its endeffector are inserted through a body wall of a patient. In someexamples, the position may optionally correspond to a remote center ofmotion for the repositionable arm. In some examples, the virtualboundary may optionally be implemented using a predetermined distance ofzero and/or a negative predetermined distance so that collisions arepermitted, but where at least some feedback is desired to limit theseverity and/or extent of the collision.

In some embodiments, method 400 is repeated to generate a virtual modelfor each of the repositionable arms for which collision avoidance isdesired. In the examples, of FIG. 1, method 400 would be repeated foreach of the repositionable arms 120 to generate a corresponding one ofthe virtual models in virtual models 150.

FIG. 5 is a simplified diagram of a method of collision avoidance usingvirtual boundaries according to some embodiments. One or more of theprocesses 510-560 of method 500 may be implemented, at least in part, inthe form of executable code stored on non-transitory, tangible, machinereadable media that when run by one or more processors (e.g., theprocessor 142 in control unit 130) may cause the one or more processorsto perform one or more of the processes 510-560. In some embodiments,method 500 may optionally be performed by one or more modules orapplications, such as virtual models 150, collision engine 152, physicsengine 154, motion control module 156, and/or haptic feedback module158. In some embodiments, method 500 is usable to determine one or morevirtual collisions between virtual boundaries of two or morerepositionable arms and use those virtual boundaries to provide feedbackto the repositionable arms to help reduce and/or eliminate actualcollisions between the repositionable arms. In some embodiments, method500 is usable to provide haptic feedback to an operator of therepositionable arms to resist attempts by the operator to manipulate therepositionable arms into a configuration where an actual collisionbetween the repositionable arms occurs. In some embodiments, method 500is usable reduce the likelihood of collisions between repositionablearms 120 and/or end effectors 125 as they are being teleoperated usinginput controls 195. In some embodiments, method 500 is usable to reducethe likelihood of collisions between the repositionable arms, endeffectors, and/or instruments of a computer-assisted medical devicewhile the computer-assisted medical device is being used to perform oneor more procedures.

At a process 510, positions of the repositionable arms are determined.Collision avoidance between repositionable arms and/or their endeffectors begins by determining current positions of each of therepositionable arms and/or their end effectors for which collisionavoidance is desired. In some embodiments, data from one or more sensorsassociated with joints of each of the repositionable arms is received.The sensor data includes information on the orientation of the joints inthe repositionable arms so that the current position of therepositionable arms is determined. In some examples, one or morekinematic models and/or virtual modules, such as the virtual modelsgenerated using method 400, are used to convert the joint orientationinformation into positions of the repositionable arms. In some examples,the orientations of the joints are provided as parameters to the one ormore kinematic and/or virtual models. In some embodiments, one or moreregistration markers, fiducial markers, and/or the like mounted on therepositionable arms may alternatively be tracked using one or moretracking sensors, such as an imaging device, to supplement and/orreplace the sensor data to determine the orientations of the joints inthe repositionable arms.

At a process 520, virtual boundaries of the repositionable arms aredetermined. Once the positions of the repositionable arms are determinedduring process 510, the positions of the repositionable arms are usableto generate current positions of the virtual boundaries of each of therepositionable arms using the one or more virtual models. In someexamples, the one or more virtual modules correspond to the virtualmodels 150 and/or the virtual models generated using method 400.

At a process 530, one or more overlaps between the virtual boundariesare determined. The virtual boundaries determined for eachrepositionable arm during process 520 are compared using a collisionengine, such as collision engine 152, to determine whether there are anyoverlaps between the virtual boundaries of different repositionablearms. In some examples, because each of the virtual boundaries mayoptionally be defined using one or more convex objects, such as theconvex hulls determined during process 430, each of the convex objectsfrom each virtual boundary are compared against the convex objects foreach of the other virtual boundaries in order to find each place wherean overlap in virtual boundaries occurs. In some examples, thecomparison may optionally include an iterative comparison strategy wherea coarse approximation of the convex objects is used to quicklydetermine potential overlaps and the actual models of the convex objectsand the virtual boundaries is used to detect the actual overlaps. Insome examples, an overlap between two virtual boundaries is detectedwhenever one of the virtual boundaries intrudes into or intersects theother virtual boundary. In some examples, detection of each overlapincludes determining an amount and location of maximum overlap betweenthe two virtual boundaries as well as a direction of the maximum overlapand/or other virtual overlap properties from which a direction andmagnitude of a virtual overlap force may be determined. In someexamples, the maximum overlap is determined by finding a point on afirst one of the virtual boundaries that extends furthest into theregion defined by a second one of the virtual boundaries and noting adistance between the first virtual boundary and the second virtualboundary along a direction of a surface normal for the first virtualboundary at the maximum overlap. In some examples, the direction of thesurface normal is used to determine a direction associated with theoverlap that indicates a direction to which a feedback force should beapplied to reduce and/or eliminate the overlap.

At a process 540, each of the overlaps detected during process 530 areprocessed in turn so that the effects of each of the overlaps and theircorresponding virtual collision is used to provide feedback to reduceand/or eliminate the virtual collisions. As each overlap is processed itbecomes the current overlap.

At a process 542, a virtual overlap force for the current overlap isdetermined. The current overlap detected during process 530 is processedby a physics engine, such as physics engine 154, to determine a virtualoverlap force of the current virtual collision. In some examples, amagnitude of the virtual overlap force is based on an amount of overlapand a direction of the virtual overlap force is based on a direction ofthe maximum overlap between the virtual boundaries and/or based on asurface normal of the virtual boundaries at the overlap. In someexamples, a virtual spring model with a linear or alternatively anon-linear constant is used to determine the magnitude of the virtualoverlap force. In some examples a deformable material model, such a gasor fluid dynamics model, for the regions within the virtual boundariesso that the volume of the overlap in combination with a coefficient ofresiliency for virtual material in the regions within the virtualboundaries provides a displacing force from which the virtual overlapforce is derived. Examples of models generating virtual feedback forcesare described in further detail in International Patent Publication No.WO 2015/120008 entitled “System and Method for Dynamic Virtual CollisionObjects,” which is hereby incorporated by reference in its entirety.

At a process 544, virtual torques on proximal joints are determined.Using the virtual overlap force determined during process 542, acorresponding virtual torque in each joint proximal to the overlap inboth repositionable arms associated with the current virtual collisionare determined. In some examples, a respective Jacobian transposebetween the point of overlap and the proximal joints is used to map thevirtual overlap force to the virtual torques. In this embodiment,computation of the virtual torques is limited to those joints proximalto the overlap as the overlap can be satisfactorily reduced and/oreliminated using only joints proximal to the point of the virtualcollision; in other embodiments, computation of the virtual torques aredetermined for other joints as well In some examples, the kinematics ofthe repositionable arm may sometimes result in no virtual torque beingcomputed for one or more of the joints proximal to the overlapindicating that manipulation of those joints does not contribute to thereduction or elimination of the overlap. Virtual torques are determinedseparately for each of the repositionable arms associated with thecurrent virtual collision as the current virtual collision can occur atdifferent locations on the two repositionable arms and the tworepositionable arms can have different kinematic models. In someexamples, the determination of the virtual torques on the proximaljoints may optionally be performed by either the physics engine and/orthe motion control module.

At a process 546, virtual tip forces are determined. Using the virtualtorques determined during process 544, a corresponding virtual tip forcefor each of the repositionable arms associated with the current virtualcollisions is determined. Each corresponding virtual tip forcerepresents an effective force that can be applied to the distal-most endof the corresponding repositionable arm to move the repositionable armaway from the current overlap. In some examples, the virtual torques foreach repositionable arm are mapped to the corresponding virtual tipforce using an inverse of a corresponding Jacobian transpose between thedistal-most end of the corresponding repositionable arm and the jointsof the corresponding repositionable arm.

At a process 548, the effects of the current overlap are superimposedwith the effects of previously processed overlaps. In some examples, thevirtual tip force determined during process 546 for each of therepositionable arms is superimposed (e.g., added to) any previousvirtual tip force determined for the corresponding repositionable arms.Thus, at process 548 the virtual tip force is adjusted with each currentoverlap and corresponding current virtual collision that thecorresponding repositionable arm is subject to, to generate an overallvirtual tip force for each of the repositionable arms. In some examples,the superposition of the effects of the overlaps may optionally beperformed by either the physics engine and/or the motion control module.

Processes 542-548 are then repeated for each of the overlaps detectedduring process 530.

At a process 550, feedback forces are applied. The overall virtual tipforces generated during process 548 are applied as a feedback force toeach of the repositionable arms. In some examples, the overall virtualtip force for each of the repositionable arms is provided to a motioncontrol application, such as a motion control application comprising amotion control module (e.g., motion control module 156), for thecorresponding repositionable arm. The motion control application appliesthe overall virtual tip force as a feedback to the motion control of thecorresponding repositionable arm so that the correspondingrepositionable arm opposes the virtual collisions detected for thecorresponding repositionable arm. In this way, motion control module isable to apply a resisting effect that helps prevent the correspondingrepositionable arm from being teleoperated closer to an actualcollision, but does not oppose motion that moves the correspondingrepositionable arm from an impending collision. In some examples, thefeedback of the corresponding virtual tip force is superimposed on themotion dictated by the teleoperated commands for the correspondingrepositionable arm.

At a process 560, haptic feedback forces are applied. The overall tipforces generated during process 548 are applied as haptic feedback tocorresponding input controls, such as input controls 195, used toteleoperate the corresponding repositionable arms. In some examples,each of the overall tip forces is scaled to reflect differences inrelative force used to manipulate the corresponding repositionable armand the force used to manipulate the corresponding input control. Insome examples, an inverse of a Jacobian transpose for the correspondinginput control is used to map the overall tip force to respective jointtorques in the repositionable arm of the input control to apply hapticfeedback to the corresponding input control. In some embodiments,differences between actual positions of the corresponding repositionablearm and a commanded position of the corresponding repositionable arm areoptionally used to determine the haptic feedback on the correspondinginput control. In some embodiments, differences between a currentcommand to the corresponding repositionable arm as adjusted by thefeedback forces applied during process 550 and a commanded position ofthe corresponding repositionable arm from the corresponding inputcontrol are optionally used to determine the haptic feedback on thecorresponding input control.

Processes 510-560 are then repeated at suitable intervals, such asduring a control loop for a device having the repositionable arms, toprovide continuous monitoring of overlaps between the virtual boundariesthat indicate virtual collisions and to provide feedback to therepositionable arms and haptic feedback to the input controls.

As discussed above and further emphasized here, FIG. 5 is merely anexample which should not unduly limit the scope of the claims. One ofordinary skill in the art would recognize many variations, alternatives,and modifications. In some embodiments, when no overlaps are detectedduring process 530, processes 540-560 are skipped and no feedback forcesand/or haptic feedback forces are applied. In some embodiments, theeffects of each of the overlaps may optionally be superimposed indifferent ways. In some examples, the virtual torques determined duringprocess 544 may be superimposed on a joint-by-joint basis to determinean overall virtual torque for each of the joints in the repositionablearms due to the overlaps and corresponding virtual collisions detectedduring process 530. The overall virtual torques are then provided to themotion control application where they are superimposed on the jointtorques due to other control inputs, such as the teleoperation commandsfrom the input controls.

In some embodiments, the effects due to redundant degrees of freedom inthe repositionable arms may optionally be accounted for when applyingthe feedback and/or haptic feedback forces. Because a repositionable armwith redundant degrees of freedom in its motion may includerepositionable element and/or joint motions that do not affect the poseof the tip of the repositionable arm, motions of a repositionable armare dividable into two orthogonal sets of motion: those that result in achange in the pose of the tip and those that do not result in a changein the pose of the tip and are sometimes referred to as null spacemotions. To address this, feedback forces applied to the repositionableelements and/or joints of the repositionable arm that are null spacemotions may optionally be omitted as they do not result in motion of thetip and consequently do not generate haptic feedback motion.

In some embodiments, it may be desirable to slowly phase in the feedbackeffects of method 500. In some examples, transition of an operating modeof a computer-assisted medical device from a mode where method 500 isnot used and a mode where method 500 is used may result in anundesirable risk of end effector and/or repositionable arm motion. Insome examples, when there is a pre-existing overlap between tworepositionable arms when the transition between mode occurs, the rapidapplication of the feedback forces during process 550 and/or hapticfeedback forces during process 560 when no such forces were previouslyapplied, this may result in unexpected and/or undesirable motion in theend effectors and/or repositionable arms. In some examples, this mayoptionally be addressed by phasing in the feedback and haptic feedbackforces upon initial transition into the mode where method 500 isperformed. In some examples, the feedback forces and/or the hapticfeedback forces may optionally be phased in using ramps, a sequence ofsteps, and/or the like. In some examples, the feedback forces and/orhaptic forces may optionally be phased in over a predetermined period oftime, such as approximately one second.

Some examples of control units, such as control unit 130 may includenon-transitory, tangible, machine readable media that include executablecode that when run by one or more processors (e.g., processor 142) maycause the one or more processors to perform the processes of methods 400and/or 500. Some common forms of machine readable media that may includethe processes of methods 400 and/or 500 are, for example, floppy disk,flexible disk, hard disk, magnetic tape, any other magnetic medium,CD-ROM, any other optical medium, punch cards, paper tape, any otherphysical medium with patterns of holes, RAM, PROM, EPROM, FLASH-EPROM,any other memory chip or cartridge, and/or any other medium from which aprocessor or computer is adapted to read.

Although illustrative embodiments have been shown and described, a widerange of modification, change and substitution is contemplated in theforegoing disclosure and in some instances, some features of theembodiments may be employed without a corresponding use of otherfeatures. One of ordinary skill in the art would recognize manyvariations, alternatives, and modifications. For example, an analogoustechnique can be applied to determine one or more other virtualboundaries around other objects other than repositionable arms, endeffectors, or arm-and-end-effector assemblies. Such other objects caninclude another part of device 110, a part of operator workstation 170,other equipment such as operating beds, carts, furniture, walls, people,etc. The process used to detect and respond to virtual collisions by arepositionable arm, end effector, or arm-and-end-effector assembly withsuch other virtual boundary can be the same for such virtually collidingarm, end effector, or arm-and-end-effector assembly as described herein.

Thus, the scope of the invention should be limited only by the followingclaims, and it is appropriate that the claims be construed broadly andin a manner consistent with the scope of the embodiments disclosedherein.

1. A computer-assisted medical device comprising: a first repositionablearm having a plurality of first joints, a plurality of first links, anda first distal end configured to support a first instrument; a secondrepositionable arm having a plurality of second joints, a plurality ofsecond links, and a second distal end configured to support a secondinstrument; a first input control configured to provide movementcommands for the first instrument; and a control unit comprising one ormore processors, the control unit coupled to the first repositionablearm, the second repositionable arm, and the first input control, whereinthe control unit: determines first joint positions of the plurality offirst joints and second positions of the plurality of second joints;uses a first virtual model of the first repositionable arm and the firstjoint positions to determine a plurality of first virtual boundariesaround the first repositionable arm; uses a second virtual model of thesecond repositionable arm and the second joint positions to determine aplurality of second virtual boundaries around the second repositionablearm; determines a first overlap between a first one of the plurality offirst virtual boundaries and a first one of the plurality of secondvirtual boundaries; determines a first overlap force on the firstrepositionable arm due to the first overlap; maps the first overlapforce to first virtual torques on multiple first joints of the pluralityof first joints; determines a first tip force on a distal end of thefirst instrument based on the first virtual torques; and applies thefirst tip force as a first feedback force on the first instrument andthe first repositionable arm.
 2. (canceled)
 3. The computer-assistedmedical device of claim 1, wherein the multiple first joints consists offirst joints of the plurality of first joints proximal to the firstoverlap.
 4. The computer-assisted medical device of claim 1, wherein thecontrol unit further: applies a first haptic feedback force to the firstinput control by using the first tip force.
 5. (canceled)
 6. Thecomputer-assisted medical device of claim 1, wherein the control unitfurther: applies a first haptic feedback force to the first inputcontrol based on a difference between a first commanded position of thefirst instrument due to application of the first feedback force on thefirst instrument and a second commanded position of the first instrumentdetermined from the first input control.
 7. The computer-assistedmedical device of claim 1, wherein the control unit further: determinesa second overlap between a second one the plurality of first virtualboundaries and a second one of the plurality of second virtualboundaries; determines a second overlap force on the firstrepositionable arm due to the second overlap; maps the second overlapforce to second virtual torques on first joints of the plurality offirst joints proximal to a second overlap position of the secondoverlap; superimposes the first and second virtual torques to producesuperimposed first and second virtual torques; and determines the firsttip force on the distal end of the first instrument based on the firstvirtual torques by using the superimposed first and second virtualtorques.
 8. The computer-assisted medical device of claim 1, wherein:the first virtual model comprises a plurality of convex hulls thatapproximates a physical geometry of the first repositionable arm; andeach boundary of the plurality of first virtual boundaries correspondsto a respective one of the convex hulls expanded in size by a respectivedistance.
 9. The computer-assisted medical device of claim 8, whereinthe respective distance of each boundary of the plurality of firstvirtual boundaries is based on a kinematic calibration of the firstrepositionable arm.
 10. (canceled)
 11. The computer-assisted medicaldevice of claim 8, wherein the respective distance of a boundary of theplurality of first virtual boundaries is zero.
 12. (canceled)
 13. Thecomputer-assisted medical device of claim 8, wherein the respectivedistance is shorter for a boundary of the plurality of the first virtualboundaries located more proximally to a mechanical ground of the firstrepositionable arm.
 14. The computer-assisted medical device of claim 1,wherein the first overlap force is mapped to the first virtual torquesusing a Jacobian transpose of the first repositionable arm, the Jacobiantranspose being between a first overlap position of the first overlap tothe first joints of the plurality of first joints proximal to the firstoverlap position.
 15. The computer-assisted medical device of claim 1,wherein the first tip force is further determined using an inverse of aJacobian transpose of the first repositionable arm, the Jacobiantranspose being between the distal end of the first instrument and theplurality of first joints.
 16. (canceled)
 17. The computer-assistedmedical device of claim 1, wherein the plurality of first virtualboundaries only apply to portions of the first repositionable arm andthe first instrument proximal to a body opening where the firstinstrument is inserted into a patient.
 18. The computer-assisted medicaldevice of claim 1, wherein the plurality of first virtual boundariesonly apply to portions of the first repositionable arm and the firstinstrument proximal to a remote center of motion of the firstrepositionable arm and the first instrument.
 19. The computer-assistedmedical device of claim 1, wherein the first overlap force is determinedfrom the first overlap by using a virtual spring model or a deformablematerial model.
 20. (canceled)
 21. The computer-assisted medical deviceof claim 1, wherein the first overlap corresponds to a largest overlapbetween the first one of the plurality of first virtual boundaries andthe first one of the plurality of second virtual boundaries. 22-23.(canceled)
 24. The computer-assisted medical device of claim 1, whereina magnitude of the first overlap force is determined based on a maximumoverlap of the first overlap, a surface area of the first overlap, or avolume of the first overlap.
 25. The computer-assisted medical device ofclaim 1, wherein the control unit further determines a surfaceinteraction force at the first overlap, the surface interaction forceperpendicular to the first overlap force.
 26. (canceled)
 27. Thecomputer-assisted medical device of claim 1, wherein the first feedbackforce is superimposed on forces applied to the first instrument as aresult of movement commands received from the first input control. 28.(canceled)
 29. The computer-assisted medical device of claim 1, whereinin the first feedback force opposes motion of the first repositionablearm that is likely to cause a collision between the first repositionablearm and the second repositionable arm.
 30. (canceled)
 31. A method ofcollision avoidance, the method comprising: determining, by a controlunit, first positions of a plurality of first joints of a firstrepositionable arm, a first instrument being mounted to a distal end ofthe first repositionable arm; determining, by the control unit, secondpositions of a plurality of second joints of a second repositionablearm, a second instrument being mounted to a distal end of the secondrepositionable arm; determining, by the control unit, positions of aplurality of first virtual boundaries around the first repositionablearm using the first positions and a first virtual model of the firstrepositionable arm; determining, by the control unit, positions of aplurality of second virtual boundaries around the second repositionablearm using the second positions and a second virtual model of the secondrepositionable arm; determining, by the control unit, a first overlapbetween a first one of the first virtual boundaries and a first one ofthe second virtual boundaries; determining, by the control unit, a firstoverlap force on the first and second repositionable arms due to thefirst overlap; mapping, by the control unit, the first overlap force tofirst virtual torques on first joints of the plurality of first jointsproximal to a first overlap position of the first overlap; determining,by the control unit, a first tip force on a distal end of the firstinstrument based on the first virtual torques; applying the first tipforce as a first feedback force on the first instrument and the firstrepositionable arm; and applying the first tip force as a first hapticfeedback force to a first input control configured to provide movementcommands for the first instrument.
 32. The method of claim 31, furthercomprising: determining, by the control unit, a second overlap between asecond one the first virtual boundaries and a second one of the secondvirtual boundaries; determining, by the control unit, a second overlapforce on the first repositionable arm due to the second overlap;mapping, by the control unit, the second overlap force to second virtualtorques on first joints of the plurality of first joints proximal to asecond overlap position of the second overlap; and superimposing, by thecontrol unit, the first and second virtual torques to superimposed firstand second virtual torques; wherein determining, by the control unit,the first tip force on the distal end of the first instrument based onthe first virtual torques comprises using the superimposed first andsecond virtual torques.
 33. (canceled)
 34. The method of claim 31,wherein mapping the first overlap force to the first virtual torquescomprises using a Jacobian transpose of the first repositionable arm,the Jacobian transpose being between the first overlap position to firstjoints of the plurality of first joints that are proximal to the firstoverlap position, or wherein determining the first tip force comprisesusing an inverse of a Jacobian transpose of the first repositionablearm, the Jacobian transpose being between the distal end of the firstinstrument and the plurality of first joints.
 35. (canceled)
 36. Themethod of claim 31, further comprising determining the first hapticfeedback force based on a difference between a first commanded positionof the first instrument due to application of the first feedback forceon the first instrument and a second commanded position of the firstinstrument determined from the first input control.
 37. Acomputer-assisted manipulation device comprising: a repositionable armhaving a plurality of joints, a plurality of links, and a distal endconfigured to support an instrument; an input control configured toprovide movement commands for the instrument; and a control unitcomprising one or more processors, the control unit coupled to therepositionable arm and the input control, wherein the control unit isconfigured to: determine joint positions of the plurality of joints; usea virtual model of the repositionable arm and the joint positions todetermine a plurality of virtual boundaries, the plurality of virtualboundaries around the repositionable arm or the instrument; determine anobject virtual boundaries around an object; determine an overlap betweenthe object virtual boundary and a virtual boundary of the plurality ofvirtual boundaries; determine an overlap force on the repositionable armdue to the overlap; map the overlap force to a virtual torque on a jointof the plurality of joints; determine a tip force on a distal end of theinstrument using the virtual torque; and apply the tip force as afeedback force on the instrument and the repositionable arm. 38-39.(canceled)
 40. The computer-assisted manipulation device of claim 37,wherein the control unit is configured to map the overlap force to thevirtual torque on a joint of the plurality of joints by: mapping theoverlap force to a plurality of virtual torques on multiple joints ofthe plurality of joints, the plurality of virtual torques comprising thevirtual torque and the multiple joints comprising the joint, and whereinthe control unit is configured to determine the tip force on the distalend of the instrument using the virtual torque by: determining the tipforce on the distal end of the instrument using the plurality of virtualtorques.
 41. The computer-assisted manipulation device of claim 37,wherein the control unit is configured to map the overlap force to thevirtual torque on a joint of the plurality of joints by: mapping theoverlap force to a plurality of virtual torques on multiple joints ofthe plurality of joints, the plurality of virtual torques comprising thevirtual torque and the multiple joints comprising the joint, and whereinthe multiple joints consists of joints of the plurality of jointsproximal to the overlap.
 42. The computer-assisted manipulation deviceof claim 37, wherein the control unit is further configured to: apply ahaptic feedback force to the input control by using the tip force.